package jp.seraph.jspf.utils;

import java.util.Map.Entry;

import jp.seraph.jsade.math.Angle;
import jp.seraph.jsade.model.AngleVelocityCalculator;
import jp.seraph.jsade.model.DefaultAngleVelocityCalculator;
import jp.seraph.jsade.model.JointIdentifier;
import jp.seraph.jspf.pose.DefaultPose;
import jp.seraph.jspf.pose.DefaultPoseElement;
import jp.seraph.jspf.pose.DefaultPoseSet;
import jp.seraph.jspf.pose.Pose;
import jp.seraph.jspf.pose.PoseElement;
import jp.seraph.jspf.pose.PoseSet;

public class PoseUtil {
    public PoseUtil(SymmetricJointContext aSymmetricContext) {
        mSymmetricContext = aSymmetricContext;
    }

    private SymmetricJointContext mSymmetricContext;

    public Pose getMirrorPose(Pose aPose) {
        return PoseUtil.getMirrorPose(aPose, mSymmetricContext);
    }

    public Pose getLeftToRightCopiedPose(Pose aPose) {
        return PoseUtil.getLeftToRightCopiedPose(aPose, mSymmetricContext);
    }

    public Pose getRightToLeftCopiedPose(Pose aPose) {
        return PoseUtil.getRightToLeftCopiedPose(aPose, mSymmetricContext);
    }

    public static Pose getMirrorPose(Pose aPose, SymmetricJointContext aContext) {
        DefaultPose tResult = new DefaultPose();
        for (Entry<JointIdentifier, Angle> tEntry : aPose) {
            tResult.setJointAngle(aContext.getSymmetricIdentifier(tEntry.getKey()), aContext.getSymmetricAngle(tEntry.getKey(), tEntry.getValue()));
        }

        return tResult;
    }

    public static Pose getLeftToRightCopiedPose(Pose aPose, SymmetricJointContext aContext) {
        DefaultPose tResult = new DefaultPose();

        for (Entry<JointIdentifier, Angle> tEntry : aPose) {
            JointIdentifier tIdentifier = tEntry.getKey();
            Angle tAngle = tEntry.getValue();
            if (aContext.isLeft(tIdentifier)) {
                tResult.setJointAngle(aContext.getSymmetricIdentifier(tIdentifier), aContext.getSymmetricAngle(tIdentifier, tAngle));
                tResult.setJointAngle(tIdentifier, tAngle);
            } else if (aContext.isCenter(tIdentifier)) {
                tResult.setJointAngle(tIdentifier, tAngle);
            }
        }

        return tResult;
    }

    public static Pose getRightToLeftCopiedPose(Pose aPose, SymmetricJointContext aContext) {
        DefaultPose tResult = new DefaultPose();

        for (Entry<JointIdentifier, Angle> tEntry : aPose) {
            JointIdentifier tIdentifier = tEntry.getKey();
            Angle tAngle = tEntry.getValue();
            if (aContext.isRight(tIdentifier)) {
                tResult.setJointAngle(aContext.getSymmetricIdentifier(tIdentifier), aContext.getSymmetricAngle(tIdentifier, tAngle));
                tResult.setJointAngle(tIdentifier, tAngle);
            } else if (aContext.isCenter(tIdentifier)) {
                tResult.setJointAngle(tIdentifier, tAngle);
            }
        }

        return tResult;
    }

    public static PoseSet createPoseSet(Pose aPose){
        return createPoseSet(aPose, new DefaultAngleVelocityCalculator());
    }

    public static PoseSet createPoseSet(Pose aPose, AngleVelocityCalculator aCalculator) {
        PoseElement tElement = new DefaultPoseElement(aPose, aCalculator);

        return new DefaultPoseSet(tElement);
    }
}
